//
// Created by sukai on 22-9-19.
//

#include <iostream>
#include <ros/ros.h>
#include <signal.h>
#include "contnav_msgs/Dictionaries.h"
#include "contnav_msgs/DictionariesVector.h"
#include "contnav_srvs/TABLE_CONTORLLER.h"
#include "common.h"
#include <queue>
using namespace std;
/**
 *
 * @param argc
 * @param argv
 * @return
 * 测试节点
 * 2022-09-19
 * sukai
 * rosrun contnav test_node
 */



map<string, string> dictionariesMap_;




//ctrl + c
void mySigintHandler(int sig)
{
    std::cout << "ctrl c  sig: "<<sig<<std::endl;

    ros::shutdown();
    if (ros::isShuttingDown()){
        cout<<"close ros  test.cpp"<<endl;
    }
}


// rosrun contnav test1_node
int main(int argc,char *argv[]){
    //节点名
    string nodeName = "test1_contnav_contnav_global_planner_node";
    //初始化节点
    ros::init(argc,argv,nodeName);
    //创建节点
    ros::NodeHandle node;
    //监控 ctrl c
    signal(SIGINT, mySigintHandler);



    vector<int> datas;//数据源
    datas.push_back((int)0xfe);
    datas.push_back((int)0x10);
    datas.push_back((int)0x15);
    datas.push_back((int)0x16);
    datas.push_back((int)0x17);

    //记录16进制
    unsigned char ss[2000];
    string ss1;
    for (int i = 0; i < datas.size(); i++)
    {
        //===========//记录bao================
        sprintf((char *)(ss+i*3),"%02x ",(datas[i] & 0x000000ff));

        //===============================
    }
    //============//记录bao=================
    for(uint16_t i=0;i<strlen((const char*)ss);i++){
        ss1 +=ss[i];
    }
    cout<<ss1<<endl;//可以
//========================================================

    vector<char> datas6;//数据源
    datas.push_back(0xfe);
    datas.push_back(0x10);
    datas.push_back(0x15);
    datas.push_back(0x16);
    datas.push_back(0x17);

    //记录16进制
    unsigned char ss6[2000];
    string ss16;
    for (int i = 0; i < datas6.size(); i++)
    {
        //===========//记录bao================
        sprintf((char *)(ss6+i*3),"%02x ",(datas6[i]));

        //===============================
    }
    //============//记录bao=================
    for(uint16_t i=0;i<strlen((const char*)ss6);i++){
        ss16 +=ss6[i];
    }
    cout<<ss16<<endl;//不可以
//====================================================
    unsigned   char dd[7]={
            0x01,0x04,0xde,0xcb,0x44,0x56,0xfd
    };
    unsigned char ss7[1000];
    string ss17;

    for(uint16_t i=0;i<7;i++){
        sprintf((char *)(ss7+i*3),"%02x ",dd[i]);
    }

    for(uint16_t i=0;i<strlen((const char*)ss7);i++){
        ss17 +=ss7[i];
    }
    cout<<ss17<<endl;//可以

    //事件轮询
    ros::spin();
    return 0;
}


